/*
 * MotionDetection.c
 *
 *  Created on: 30/nov/2010
 *      Author: fabiofurlan
 */

#include "cv.h"
#include "highgui.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>

#define BS 11

int main(void) {
	IplImage *imgA = NULL;
	IplImage *imgB = NULL;
	IplImage *grayA = NULL;
	IplImage *grayB = NULL;
	IplImage *velx = NULL;
	IplImage *vely = NULL;
	int aux = 0;

	CvCapture * pCapture; //new OpenCV capture stream
	IplImage *frame, *frame_copy, *frame_flip = 0;

	pCapture = cvCaptureFromCAM(0); //choose camera for capture
	cvSetCaptureProperty(pCapture, CV_CAP_PROP_FPS, 15);
	cvNamedWindow("video");
	cvResizeWindow("video", 640, 480);

	imgA = cvQueryFrame(pCapture);
	imgB = cvQueryFrame(pCapture);

	while (aux!=-1) {

		printf("Iterazione Test\n");
		grayA = cvCreateImage(cvGetSize(imgA), IPL_DEPTH_8U, 1);
		grayB = cvCreateImage(cvGetSize(imgB), IPL_DEPTH_8U, 1);

		CvSize size = cvGetSize(imgA);

		velx = cvCreateImage(size, IPL_DEPTH_32F, 1); // cvCreateImage(cvGetSize(imgA),32,1);
		vely = cvCreateImage(size, IPL_DEPTH_32F, 1);

		cvCvtColor(imgA, grayA, CV_BGR2GRAY);
		cvCvtColor(imgB, grayB, CV_BGR2GRAY);

		cvCalcOpticalFlowLK(grayB, grayA, cvSize(BS, BS), velx, vely);

		//cvNamedWindow("HorFlowBM", CV_WINDOW_AUTOSIZE);
		//cvShowImage("HorFlowBM", velx);
		//cvNamedWindow("VerFlowBM", CV_WINDOW_AUTOSIZE);
		//cvShowImage("VerFlowBM", vely);

		for (int i = 0; i < imgA->height; i += 5) {
			for (int j = 0; j < imgA->width; j += 5) {
				int dx = (int) cvGetReal2D(velx, i, j);
				int dy = (int) cvGetReal2D(vely, i, j);
				cvLine(imgA, cvPoint(j, i), cvPoint(j + dx, i + dy), CV_RGB(
						255, 255, 255), 1, 8, 0);
			}
		}
		cvShowImage("video", imgA);

		imgB=cvQueryFrame(pCapture);
		// Wait for a while before proceeding to the next frame
		if (cvWaitKey(67) >= 0)
				break;
		imgA=cvQueryFrame(pCapture);

	}
}
